#include "Quaternion_to_R.h"

void Quaternion_to_R_Matrix(Q_info_Typedef *Q , Matrix_to_R_Typdef *R_out)
{
    float q0 = Q->q0;
    float q1 = Q->q1;
    float q2 = Q->q2;
    float q3 = Q->q3;

    R_out->Matrix_to_R[0] = 1 - 2*q2*q2 - 2*q3*q3;
    R_out->Matrix_to_R[1] = 2*q1*q2 - 2*q3*q0;
    R_out->Matrix_to_R[2] = 2*q1*q3 + 2*q2*q0;

    R_out->Matrix_to_R[3] = 2*q1*q2 + 2*q3*q0;
    R_out->Matrix_to_R[4] = 1 - 2*q1*q1 - 2*q3*q3;
    R_out->Matrix_to_R[5] = 2*q2*q3 - 2*q1*q0;

    R_out->Matrix_to_R[6] = 2*q1*q3 - 2*q2*q0;
    R_out->Matrix_to_R[7] = 2*q2*q3 + 2*q1*q0;
    R_out->Matrix_to_R[8] = 1 - 2*q1*q1 - 2*q2*q2;
}